{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from numpy import linalg as LA\n",
    "from numpy.linalg import inv\n",
    "import math\n",
    "from numpy import lexsort\n",
    "\n",
    "import matplotlib as mpl\n",
    "from matplotlib import pyplot as plt\n",
    "from matplotlib import cm\n",
    "import matplotlib.patches as mpatches\n",
    "\n",
    "from sklearn import metrics\n",
    "from sklearn.metrics import average_precision_score\n",
    "from sklearn.metrics import precision_recall_curve\n",
    "\n",
    "# Load data data\n",
    "ground_truth_poses = np.loadtxt(\"../data/experiments/pose_scanner_leica_affine.txt\") # id,x,y,z,w,x,y,z\n",
    "ground_truth_poses = np.array([ground_truth_poses]).T        \n",
    "\n",
    "corrected_poses = np.loadtxt(\"../data/experiments/corrected_poses_360fov_1200_run2.txt\")              # idA,idB,x,y,z,w,x,y,z\n",
    "corrected_poses = np.array([corrected_poses]).T\n",
    "\n",
    "results = np.loadtxt(\"../data/experiments/compare_results_360fov_1200_run2.txt\")                     # idA,idB,\n",
    "results = np.array([results]).T                                                 # fov_overlap, octree_overlap\n",
    "                                                                                    # alignability, alignment_risk\n",
    "                                                                                    # degeneracy, ICN\n",
    "# ground_truth_poses\n",
    "# print results"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Compute translation error\n",
    "def translationError(xg, yg, zg, xc, yc, zc):\n",
    "   \n",
    "    trans_err = np.zeros((xc.shape[0],1))\n",
    "    gt_index = 0\n",
    "    for i in range(0, xc.shape[0]):\n",
    "        if (gt_index > xg.shape[0]-1):\n",
    "            gt_index = 0\n",
    "        \n",
    "#         print gt_index, i\n",
    "  \n",
    "        trans_err[i] =  math.sqrt( pow((xg[gt_index] - xc[i]),2.0) + \n",
    "                                   pow((yg[gt_index] - yc[i]),2.0) +\n",
    "                                   pow((zg[gt_index] - zc[i]),2.0) );\n",
    "        gt_index = gt_index+1\n",
    "    return trans_err\n",
    "49\n",
    "transl_err = translationError(ground_truth_poses[1], # [col][row]\n",
    "                              ground_truth_poses[2],\n",
    "                              ground_truth_poses[3],\n",
    "                              corrected_poses[2],\n",
    "                              corrected_poses[3],\n",
    "                              corrected_poses[4])\n",
    "\n",
    "# transl_err"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Create Labels given ground truth and errors\n",
    "transl_thresh = 0.02 # [m]\n",
    "rot_thresh = 30\n",
    "\n",
    "def createLabels(transl_err):\n",
    "    \n",
    "    labels = np.zeros((transl_err.shape[0],1))\n",
    "\n",
    "    for i in range(0, labels.shape[0]):\n",
    "        if transl_err[i] > transl_thresh: #and rot_err[i] > rot_thresh:\n",
    "            labels[i] = 1.0\n",
    "\n",
    "    return labels\n",
    "\n",
    "labels = createLabels(transl_err)\n",
    "\n",
    "# print labels.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "probs_ours = results[5]\n",
    "probs_degeneracy = results[6]\n",
    "probs_icn = results[7]\n",
    "\n",
    "risk_threshold = 0.50\n",
    "\n",
    "overlap = results[3]\n",
    "shape = overlap.shape\n",
    "num_bins = 10\n",
    "# Accuracy vs. Overlap\n",
    "bin_acc_ours = np.zeros((num_bins,1))\n",
    "for i in range(1, 11):\n",
    "    bin_max = num_bins*i\n",
    "    count = 0\n",
    "    true = 0\n",
    "#     print 'bin_max =', bin_max\n",
    "    for j in range(0, shape[0]):\n",
    "#         print 'j =', j\n",
    "#         print 'overlap[j] =', overlap[j], overlap[j] <= bin_max\n",
    "        if (overlap[j] <= bin_max and overlap[j] > (bin_max-num_bins)):\n",
    "            count = count + 1\n",
    "#             print 'labels[j] =', labels[j], probs_ours[j]>risk_threshold, probs_ours[j]<=risk_threshold\n",
    "            if (labels[j]==1.0 and probs_ours[j]>risk_threshold) or (labels[j]==0.0 and probs_ours[j]<=risk_threshold):\n",
    "#             if (labels[j]==1.0 and probs_ours[j]<risk_threshold):\n",
    "                # true positive or true negative\n",
    "                true = true + 1\n",
    "    bin_acc_ours[i-1] = 100.0*(float(true) / float(count))\n",
    "    \n",
    "# print bin_acc_ours"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 1.        ]\n",
      " [ 0.9071618 ]\n",
      " [ 0.72727273]\n",
      " [ 0.4057971 ]\n",
      " [ 0.34782609]\n",
      " [ 0.2       ]\n",
      " [ 1.        ]\n",
      " [ 1.        ]\n",
      " [ 1.        ]\n",
      " [ 1.        ]]\n"
     ]
    }
   ],
   "source": [
    "# Precision-Recall vs. Overlap\n",
    "bin_recall_ours = np.zeros((num_bins,1))\n",
    "bin_precision_ours = np.zeros((num_bins,1))\n",
    "bin_fscore_ours = np.zeros((num_bins,1))\n",
    "# Recall\n",
    "for i in range(1, 11):\n",
    "    bin_max = num_bins*i\n",
    "    cond_positive = 0\n",
    "    true = 0\n",
    "    for j in range(0, shape[0]):\n",
    "        if (overlap[j] <= bin_max and overlap[j] > (bin_max-num_bins)):\n",
    "            if (labels[j]==1.0):\n",
    "                # condition positive\n",
    "                cond_positive = cond_positive + 1\n",
    "                if (probs_ours[j]>risk_threshold):\n",
    "                    # true positive\n",
    "                    true = true + 1\n",
    "#     print float(true)\n",
    "#     print float(cond_positive)\n",
    "    if cond_positive>0:\n",
    "        bin_recall_ours[i-1] = (float(true) / float(cond_positive))\n",
    "    \n",
    "# print bin_recall_ours\n",
    "\n",
    "# Precision\n",
    "for i in range(1, 11):\n",
    "    bin_max = num_bins*i\n",
    "    pred_cond_positive = 0\n",
    "    true = 0\n",
    "    for j in range(0, shape[0]):\n",
    "        if (overlap[j] <= bin_max and overlap[j] > (bin_max-num_bins)):\n",
    "            if (probs_ours[j]>risk_threshold):\n",
    "                # predicted condition positive\n",
    "                pred_cond_positive = pred_cond_positive + 1\n",
    "                if (labels[j]==1.0):\n",
    "                    # true positive\n",
    "                    true = true + 1\n",
    "#     print float(pred_cond_positive)\n",
    "#     print float(true)\n",
    "    if true>0:\n",
    "        bin_precision_ours[i-1] = (float(true) / float(pred_cond_positive))\n",
    "    \n",
    "# print bin_precision_ours\n",
    "\n",
    "# F-Score\n",
    "for i in range(0, 10):\n",
    "    if bin_recall_ours[i]>0:\n",
    "        den1 = (1.0 / bin_recall_ours[i])\n",
    "    else:\n",
    "        den1 = 1.0\n",
    "    if bin_precision_ours[i]>0:\n",
    "        den2 = (1.0 / bin_precision_ours[i])\n",
    "    else:\n",
    "        den2 = 1.0\n",
    "    bin_fscore_ours[i] = 2.0 / ( den1 + den2 )\n",
    "    \n",
    "print bin_fscore_ours"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "deg_threshold = 0.05\n",
    "\n",
    "overlap = results[3]\n",
    "shape = overlap.shape\n",
    "num_bins = 10\n",
    "bin_acc_deg = np.zeros((num_bins,1))\n",
    "for i in range(1, 11):\n",
    "    bin_max = num_bins*i\n",
    "    count = 0\n",
    "    true = 0\n",
    "#     print 'bin_max =', bin_max\n",
    "    for j in range(0, shape[0]):\n",
    "#         print 'j =', j\n",
    "#         print 'overlap[j] =', overlap[j], overlap[j] <= bin_max\n",
    "        if (overlap[j] <= bin_max and overlap[j] > (bin_max-num_bins)):\n",
    "            count = count + 1\n",
    "#             print 'labels[j] =', labels[j], probs_degeneracy[j]>risk_threshold, probs_degeneracy[j]<=risk_threshold\n",
    "            if (labels[j]==1.0 and probs_degeneracy[j]<deg_threshold) or (labels[j]==0.0 and probs_degeneracy[j]>=deg_threshold):\n",
    "#             if (labels[j]==1.0 and probs_degeneracy[j]>deg_threshold):\n",
    "                # true positive or true negative\n",
    "                true = true + 1\n",
    "    bin_acc_deg[i-1] = 100.0*(float(true) / float(count))\n",
    "    \n",
    "# print bin_acc_deg"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 0.87651332]\n",
      " [ 0.66013072]\n",
      " [ 0.45751634]\n",
      " [ 0.53846154]\n",
      " [ 0.29411765]\n",
      " [ 0.26086957]\n",
      " [ 1.        ]\n",
      " [ 1.        ]\n",
      " [ 1.        ]\n",
      " [ 1.        ]]\n"
     ]
    }
   ],
   "source": [
    "# Precision-Recall vs. Overlap\n",
    "bin_recall_deg = np.zeros((num_bins,1))\n",
    "bin_precision_deg = np.zeros((num_bins,1))\n",
    "bin_fscore_deg = np.zeros((num_bins,1))\n",
    "# Recall\n",
    "for i in range(1, 11):\n",
    "    bin_max = num_bins*i\n",
    "    cond_positive = 0\n",
    "    true = 0\n",
    "    for j in range(0, shape[0]):\n",
    "        if (overlap[j] <= bin_max and overlap[j] > (bin_max-num_bins)):\n",
    "            if (labels[j]==1.0):\n",
    "                # condition positive\n",
    "                cond_positive = cond_positive + 1\n",
    "                if (probs_degeneracy[j]<deg_threshold):\n",
    "                    # true positive\n",
    "                    true = true + 1\n",
    "#     print float(true)\n",
    "#     print float(cond_positive)\n",
    "    if cond_positive>0:\n",
    "        bin_recall_deg[i-1] = (float(true) / float(cond_positive))\n",
    "    \n",
    "# print bin_recall_deg\n",
    "\n",
    "# Precision\n",
    "for i in range(1, 11):\n",
    "    bin_max = num_bins*i\n",
    "    pred_cond_positive = 0\n",
    "    true = 0\n",
    "    for j in range(0, shape[0]):\n",
    "        if (overlap[j] <= bin_max and overlap[j] > (bin_max-num_bins)):\n",
    "            if (probs_degeneracy[j]<deg_threshold):\n",
    "                # predicted condition positive\n",
    "                pred_cond_positive = pred_cond_positive + 1\n",
    "                if (labels[j]==1.0):\n",
    "                    # true positive\n",
    "                    true = true + 1\n",
    "#     print float(pred_cond_positive)\n",
    "#     print float(true)\n",
    "    if true>0:\n",
    "        bin_precision_deg[i-1] = (float(true) / float(pred_cond_positive))\n",
    "    \n",
    "# print bin_precision_deg\n",
    "\n",
    "# F-Score\n",
    "for i in range(0, 10):\n",
    "    if bin_recall_deg[i]>0:\n",
    "        den1 = (1.0 / bin_recall_deg[i])\n",
    "    else:\n",
    "        den1 = 1.0\n",
    "    if bin_precision_deg[i]>0:\n",
    "        den2 = (1.0 / bin_precision_deg[i])\n",
    "    else:\n",
    "        den2 = 1.0\n",
    "    bin_fscore_deg[i] = 2.0 / ( den1 + den2 )\n",
    "    \n",
    "print bin_fscore_deg"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "icn_threshold = 0.05\n",
    "\n",
    "overlap = results[3]\n",
    "shape = overlap.shape\n",
    "num_bins = 10\n",
    "bin_acc_icn = np.zeros((num_bins,1))\n",
    "for i in range(1, 11):\n",
    "    bin_max = num_bins*i\n",
    "    count = 0\n",
    "    true = 0\n",
    "#     print 'bin_max =', bin_max\n",
    "    for j in range(0, shape[0]):\n",
    "#         print 'j =', j\n",
    "#         print 'overlap[j] =', overlap[j], overlap[j] <= bin_max\n",
    "        if (overlap[j] <= bin_max and overlap[j] > (bin_max-num_bins)):\n",
    "            count = count + 1\n",
    "#             print 'labels[j] =', labels[j], probs_icn[j]>risk_threshold, probs_icn[j]<=risk_threshold\n",
    "            if (labels[j]==1.0 and probs_icn[j]<icn_threshold) or (labels[j]==0.0 and probs_icn[j]>=icn_threshold):\n",
    "#             if (labels[j]==1.0 and probs_icn[j]>icn_threshold):\n",
    "                # true positive or true negative\n",
    "                true = true + 1\n",
    "    bin_acc_icn[i-1] = 100.0*(float(true) / float(count))\n",
    "    \n",
    "# print bin_acc_icn"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 0.83709273]\n",
      " [ 0.72049689]\n",
      " [ 0.55813953]\n",
      " [ 0.52525253]\n",
      " [ 0.37037037]\n",
      " [ 0.15      ]\n",
      " [ 1.        ]\n",
      " [ 1.        ]\n",
      " [ 1.        ]\n",
      " [ 1.        ]]\n"
     ]
    }
   ],
   "source": [
    "# Precision-Recall vs. Overlap\n",
    "bin_recall_icn = np.zeros((num_bins,1))\n",
    "bin_precision_icn = np.zeros((num_bins,1))\n",
    "bin_fscore_icn = np.zeros((num_bins,1))\n",
    "# Recall\n",
    "for i in range(1, 11):\n",
    "    bin_max = num_bins*i\n",
    "    cond_positive = 0\n",
    "    true = 0\n",
    "    for j in range(0, shape[0]):\n",
    "        if (overlap[j] <= bin_max and overlap[j] > (bin_max-num_bins)):\n",
    "            if (labels[j]==1.0):\n",
    "                # condition positive\n",
    "                cond_positive = cond_positive + 1\n",
    "                if (probs_icn[j]<icn_threshold):\n",
    "                    # true positive\n",
    "                    true = true + 1\n",
    "#     print float(true)\n",
    "#     print float(cond_positive)\n",
    "    if cond_positive>0:\n",
    "        bin_recall_icn[i-1] = (float(true) / float(cond_positive))\n",
    "    \n",
    "# print bin_recall_icn\n",
    "\n",
    "# Precision\n",
    "for i in range(1, 11):\n",
    "    bin_max = num_bins*i\n",
    "    pred_cond_positive = 0\n",
    "    true = 0\n",
    "    for j in range(0, shape[0]):\n",
    "        if (overlap[j] <= bin_max and overlap[j] > (bin_max-num_bins)):\n",
    "            if (probs_icn[j]<icn_threshold):\n",
    "                # predicted condition positive\n",
    "                pred_cond_positive = pred_cond_positive + 1\n",
    "                if (labels[j]==1.0):\n",
    "                    # true positive\n",
    "                    true = true + 1\n",
    "#     print float(pred_cond_positive)\n",
    "#     print float(true)\n",
    "    if true>0:\n",
    "        bin_precision_icn[i-1] = (float(true) / float(pred_cond_positive))\n",
    "    \n",
    "# print bin_precision_icn\n",
    "\n",
    "# F-Score\n",
    "for i in range(0, 10):\n",
    "    if bin_recall_icn[i]>0:\n",
    "        den1 = (1.0 / bin_recall_icn[i])\n",
    "    else:\n",
    "        den1 = 1.0\n",
    "    if bin_precision_icn[i]>0:\n",
    "        den2 = (1.0 / bin_precision_icn[i])\n",
    "    else:\n",
    "        den2 = 1.0\n",
    "    bin_fscore_icn[i] = 2.0 / ( den1 + den2 )\n",
    "    \n",
    "print bin_fscore_icn"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 41,
   "metadata": {},
   "outputs": [],
   "source": [
    "from scipy.interpolate import spline\n",
    "from scipy.ndimage.interpolation import shift\n",
    "\n",
    "\n",
    "# plot accuracy when overlap [0:10],[10,20],...\n",
    "bins = [0,10,20,30,40,50,60,70,80,90,100,110]\n",
    "\n",
    "xnew = np.linspace(0,105,105) #300 represents number of points to make between T.min and T.max\n",
    "\n",
    "\n",
    "first_ours = np.expand_dims(bin_acc_ours[0], axis=0)\n",
    "last_ours = np.expand_dims(bin_acc_ours[9], axis=0)\n",
    "conc_ours = np.concatenate((first_ours, bin_acc_ours), axis=0)\n",
    "conc_ours = np.concatenate((conc_ours, last_ours), axis=0)\n",
    "\n",
    "first_deg = np.expand_dims(bin_acc_deg[0], axis=0)\n",
    "last_deg = np.expand_dims(bin_acc_deg[9], axis=0)\n",
    "conc_deg = np.concatenate((first_deg, bin_acc_deg), axis=0)\n",
    "conc_deg = np.concatenate((conc_deg, last_deg), axis=0)\n",
    "\n",
    "first_icn = np.expand_dims(bin_acc_icn[0], axis=0)\n",
    "last_icn = np.expand_dims(bin_acc_icn[9], axis=0)\n",
    "conc_icn = np.concatenate((first_icn, bin_acc_icn), axis=0)\n",
    "conc_icn = np.concatenate((conc_icn, last_icn), axis=0)\n",
    "\n",
    "conc_ours_smooth = spline(bins,conc_ours,shift(xnew,-5))\n",
    "conc_deg_smooth = spline(bins,conc_deg,shift(xnew,-5))\n",
    "conc_icn_smooth = spline(bins,conc_icn,shift(xnew,-5))\n",
    "\n",
    "plt.plot(xnew,conc_ours_smooth,label = 'AR',linewidth=2.0)\n",
    "plt.plot(xnew,conc_deg_smooth,label = 'D',linewidth=2.0)\n",
    "plt.plot(xnew,conc_icn_smooth,label = 'ICN',linewidth=2.0)\n",
    "\n",
    "\n",
    "\n",
    "# our_lines = plt.step(bins,conc_ours,'b',linewidth=2.0,label = 'AR', alpha=1)\n",
    "# # plt.fill_between(our_lines[0].get_xdata(orig=False), 0, our_lines[0].get_ydata(orig=False), color = 'red', alpha=1)\n",
    "# deg_lines = plt.step(bins,conc_deg,'g',linewidth=2.0,label = 'Deg', alpha=1)\n",
    "# # # plt.fill_between(deg_lines[0].get_xdata(orig=False), 0, deg_lines[0].get_ydata(orig=False), color = 'blue', alpha=0.5)\n",
    "# icn_lines = plt.step(bins,conc_icn,'r',linewidth=2.0,label = 'ICN', alpha=1)\n",
    "# # # plt.fill_between(icn_lines[0].get_xdata(orig=False), 0, icn_lines[0].get_ydata(orig=False), color = 'green', alpha=0.5)\n",
    "plt.xlabel('Overlap [%]')\n",
    "plt.ylabel('Accuracy [%]')\n",
    "plt.xticks(np.arange(0, 101, 10))\n",
    "plt.yticks(np.arange(0, 101, 10))\n",
    "plt.ylim(30,100)\n",
    "plt.xlim(0,100)\n",
    "plt.legend(loc = 'lower right')\n",
    "# plt.text(8, 90, 'AR', color='black', horizontalalignment='center', fontsize=15)\n",
    "# plt.text(8, 31, 'Deg', color='black', horizontalalignment='center', fontsize=15)\n",
    "# plt.text(8, 50, 'ICN', color='black', horizontalalignment='center', fontsize=15)\n",
    "plt.grid()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# plot fscore when overlap [0:10],[10,20],...\n",
    "bins = [0,10,20,30,40,50,60,70,80,90,100]\n",
    "\n",
    "first_ours = np.expand_dims(bin_fscore_ours[0], axis=0)\n",
    "conc_ours = np.concatenate((first_ours, bin_fscore_ours), axis=0)\n",
    "\n",
    "first_deg = np.expand_dims(bin_fscore_deg[0], axis=0)\n",
    "conc_deg = np.concatenate((first_deg, bin_fscore_deg), axis=0)\n",
    "\n",
    "first_icn = np.expand_dims(bin_fscore_icn[0], axis=0)\n",
    "conc_icn = np.concatenate((first_icn, bin_fscore_icn), axis=0)\n",
    "\n",
    "our_lines = plt.step(bins,conc_ours,'r',linewidth=2.0,label = 'AR', alpha=1)\n",
    "# plt.fill_between(our_lines[0].get_xdata(orig=False), 0, our_lines[0].get_ydata(orig=False), color = 'red', alpha=0)\n",
    "deg_lines = plt.step(bins,conc_deg,'b',linewidth=2.0,label = 'Deg', alpha=1)\n",
    "# plt.fill_between(deg_lines[0].get_xdata(orig=False), 0, deg_lines[0].get_ydata(orig=False), color = 'blue', alpha=0)\n",
    "icn_lines = plt.step(bins,conc_icn,'g',linewidth=2.0,label = 'ICN', alpha=1)\n",
    "# plt.fill_between(icn_lines[0].get_xdata(orig=False), 0, icn_lines[0].get_ydata(orig=False), color = 'green', alpha=0)\n",
    "plt.xlabel('Overlap [%]')\n",
    "plt.ylabel('F1-Score [%]')\n",
    "plt.xticks(np.arange(0, 101, 10))\n",
    "plt.yticks(np.arange(0, 1.1, 10))\n",
    "plt.ylim(0,1.1)\n",
    "# plt.legend(loc = 'lower right')\n",
    "plt.text(8, 90, 'AR', color='black', horizontalalignment='center', fontsize=15)\n",
    "plt.text(8, 31, 'Deg', color='black', horizontalalignment='center', fontsize=15)\n",
    "plt.text(8, 50, 'ICN', color='black', horizontalalignment='center', fontsize=15)\n",
    "plt.grid()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
